/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.                             */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in the root directory of */
/* the project.                                                               */
/*----------------------------------------------------------------------------*/

package edu.wpi.first.wpilibj.templates;


import edu.wpi.first.wpilibj.*;
import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.*;
import edu.wpi.first.wpilibj.image.NIVisionException;
import edu.wpi.first.wpilibj.image.RGBImage;

/**
 * The VM is configured to automatically run this class, and to call the
 * functions corresponding to each mode, as described in the IterativeRobot
 * documentation. If you change the name of this class or the package after
 * creating this project, you must also update the manifest file in the resource
 * directory.
 */
public class Camera extends IterativeRobot {
    /**
     * This function is run when the robot is first started up and should be
     * used for any initialization code.
     */
    AxisCamera camera;
    public void robotInit() {
        //camera = AxisCamera.getInstance();
    }

    /**
     * This function is called periodically during autonomous
     */
    public void autonomousPeriodic() {
        ColorImage image;
        try {
            //image = camera.getImage();
            image = new RGBImage("/ImageTest1.jpg");
            BinaryImage thresholdImage = image.thresholdHSL(0, 150, 0, 5, 250, 255);
            BinaryImage largeObjects = thresholdImage.removeSmallObjects(false, 2);
            BinaryImage convexHull = largeObjects.convexHull(false);
            ParticleAnalysisReport[] reports = convexHull.getOrderedParticleAnalysisReports();
                for (int i = 0; i < reports.length; i++) {
                ParticleAnalysisReport r = reports[i];
                System.out.println("Particle: " + i + ": Center of mass x: " + r.center_mass_x);
}   
                image.free();
                thresholdImage.free();
                largeObjects.free();
                convexHull.free();
        }// catch (AxisCameraException ex) {
          //  ex.printStackTrace();
        //}
    catch (NIVisionException ex) {
            ex.printStackTrace();
        
        }
    }

    /**
     * This function is called periodically during operator control
     */
    public void teleopPeriodic() {
        
    }
    
}
